#include "CMeasure.h"

CMeasure::CMeasure(void)
{
	//f_measure.open("measure_record.txt",ios::trunc);
}

CMeasure::~CMeasure(void)
{
}

void CMeasure::DoAct(void)
{
	timeInterval = timer.GetTimeInterval();
	timer.Start();
	if(timeInterval)
	{
		lastvelForceSet=sa->GetVelForceSet();
		lastVelMotor = sa->GetVelMotor();
		lastVelLoad = sa->GetVelLoad();
		if((sa->GetPosMotor()-lastPosMotor)>PI)
		{
			sa->SetVelMotor( ((sa->GetPosMotor()-lastPosMotor-2*PI)/timeInterval) );
		}
		else if((sa->GetPosMotor()-lastPosMotor)<-PI)
		{
			sa->SetVelMotor( ((sa->GetPosMotor()-lastPosMotor+2*PI)/timeInterval) );
		}
		else
		{
			sa->SetVelMotor( ((sa->GetPosMotor()-lastPosMotor)/timeInterval) );
		}

		if((sa->GetPosLoad()-lastPosLoad)>PI)
		{
			sa->SetVelLoad( ((sa->GetPosLoad()-lastPosLoad-2*PI)/timeInterval) );
		}
		else if((sa->GetPosLoad()-lastPosLoad)<-PI)
		{
			sa->SetVelLoad( ((sa->GetPosLoad()-lastPosLoad+2*PI)/timeInterval) );
		}
		else
		{
			sa->SetVelLoad( ((sa->GetPosLoad()-lastPosLoad)/timeInterval) );
		}

		sa->SetAccMotor( ((sa->GetVelMotor()-lastVelMotor)/timeInterval) );
		sa->SetAccLoad( ((sa->GetVelLoad()-lastVelLoad)/timeInterval) );

		sa->SetVelForceSet( ((sa->GetForceSet()-lastForceSet)/timeInterval) );
		sa->SetVelPositionSet( ((sa->GetPositionSet()-lastPositionSet)/timeInterval) );
		//sa->SetVelPositionSet( ((sa->GetVelocitySet()-lastVelocitySet)/timeInterval) );
		sa->SetAccForceSet( ((sa->GetVelForceSet()-lastvelForceSet)/timeInterval) );

		sa->SetTimeInterval(timeInterval);
	}
	else
	{
		sa->SetVelMotor(0);
		sa->SetVelLoad(0);

		sa->SetAccMotor(0);
		sa->SetAccLoad(0);

		sa->SetVelForceSet(0);
	}
	lastPosMotor = sa->GetPosMotor();
	lastPosLoad = sa->GetPosLoad();
	lastForceSet = sa->GetForceSet();
	lastPositionSet = sa->GetPositionSet();
	lastVelocitySet = sa->GetVelocitySet();

	return;
}
